using UnityEngine;
using System.Collections;

public class lwAstarWalker : MonoBehaviour
{
	//PathFinding tool
	private lwAstarGraph m_Graph = null;
	public lwAstarGraph Graph {set{m_Graph = value;}get{return m_Graph;}}

	//Position and direction updating delegate for project specific implementation
	public delegate void updatePositionDelegate(Transform WalkerTransform, Vector3 newPosition);
	public delegate void updateDirectionDelegate(Vector3 newDirection);

	public updatePositionDelegate m_UpdatePostionDelegate = null;
	public updateDirectionDelegate m_UpdateDirectionDelegate = null;

	//Walker Speed in unit per second
	public float Speed = 100;

	//link weight divider for correct anim speed of visual
	private float m_fCurrentSpeedRatio = 1;
	public float CurrentSpeedRatio {get{return m_fCurrentSpeedRatio;}}

	private lwAstarNode[] Solution = null;
	private int m_nStep = -1;
	private float m_fCostTraveledOnCurrentLink = 0;
	private lwAstarNode[] LastCanGoSolution = null;

	public lwAstarNode.PassedDelegate PassedNode = null;

	private lwAstarNode m_StartNode = null;
	private lwAstarNode m_NodeTeleportedOn = null;
	private bool m_bIsWalking = false;
	public bool IsWalking
	{
		set
		{
			m_bIsWalking = value;
//			Debug.LogWarning("Forcing m_bIsWalking value, the system hope that you know what you are doing");
		}
		get{return m_bIsWalking;}
	}

	public void SetStartNode(lwAstarNode startNode)
	{
		m_StartNode = startNode;
	}
	
	public lwAstarNode GetStartNode()
	{
		return (m_StartNode != null) ? m_StartNode : (m_Graph != null) ? m_Graph.GetStartNode() : null;
	}

	public void Teleport(lwAstarNode node)
	{
		if(m_bIsWalking)
		{
			Debug.LogError("can't use Teleport on a currently walking walker");
		}
		else
		{
			m_nStep = -1;
			m_fCostTraveledOnCurrentLink = 0;
			Solution = null;
			LastCanGoSolution = null;
			m_NodeTeleportedOn = node;
			if (m_UpdatePostionDelegate != null)
				m_UpdatePostionDelegate(transform, node.transform.localPosition);
			else
				transform.localPosition = node.transform.localPosition;
		}
	}

	public bool CanGoTo(lwAstarNode destination)
	{
		LastCanGoSolution = null;
		lwAstarNode startnode = null;
		if (m_NodeTeleportedOn!=null)
		{
			startnode = m_NodeTeleportedOn;
		}
		else if (Solution != null && m_nStep >= 0 )
		{
			if (m_nStep < Solution.Length - 2) 
            	startnode = Solution[m_nStep+1]; 
            else 
                startnode = Solution[m_nStep];
		}
		else
		{
			startnode = GetStartNode();
		}

		LastCanGoSolution = m_Graph.m_PathFinder.FindPath(startnode, destination);
		return LastCanGoSolution!=null;
	}
	
	public float GetLastCanGoSolutionCost()
	{
		if ( LastCanGoSolution	!= null )
			return LastCanGoSolution[LastCanGoSolution.Length-1].GlobalCost;
		return float.MaxValue;
	}

	public IEnumerator WalkPathTo(lwAstarNode destination)
	{
		m_bIsWalking = true;
		if (m_Graph == null || m_Graph.m_PathFinder == null || m_Graph.GetNodeCount() <= 1)
		{
			Debug.LogError("WalkPathTo call failed"
			+ ((m_Graph == null) ? " No Graph !"
			: (((m_Graph.m_PathFinder == null) ? " No PathFinder !" : "")
			+ ((m_Graph.GetNodeCount() <= 1) ? " Graph need a least 2 node!" : "")))
			);
		}
		else
		{
			lwAstarNode startNode = null;
			float now = m_Graph.GetTime();
			float fPrevTime = now;
			float deltaTime = 0;
			float cumulatedCost = 0;
			if (Solution != null)
			{
				if(/*m_fCostTraveledOnCurrentLink > 0 &&*/ m_nStep >= 0 && m_nStep<Solution.Length-1)
				{	//we are interrupting an ongoing WalkPathTo
					//we should finish the movement to next node then swap to a new path.
					lwAstarNode.lwAstarLink link = Solution[m_nStep].GetLinkTo(Solution[m_nStep+1]);
					float lastLinkCost = link.cost;

					while (lastLinkCost-m_fCostTraveledOnCurrentLink > Speed * deltaTime)
					{
						m_fCostTraveledOnCurrentLink = Mathf.Clamp( m_fCostTraveledOnCurrentLink + Speed * deltaTime, 0, lastLinkCost );
						float ratio = m_fCostTraveledOnCurrentLink / lastLinkCost;
						if (m_UpdatePostionDelegate != null)
							m_UpdatePostionDelegate(transform, Vector3.Lerp(Solution[m_nStep].transform.localPosition, Solution[m_nStep + 1].transform.localPosition, ratio));
						else
							transform.localPosition = Vector3.Lerp(Solution[m_nStep].transform.localPosition, Solution[m_nStep + 1].transform.localPosition, ratio);

						yield return null;

						now = m_Graph.GetTime();
						deltaTime = now - fPrevTime;
						fPrevTime = now;
					}
					m_nStep++;
				}
				startNode = Solution[m_nStep];
			}
			else if (m_NodeTeleportedOn!=null)
			{
				startNode = m_NodeTeleportedOn;
				m_NodeTeleportedOn = null;
			}
			
			if(startNode == null)
			{
				startNode = GetStartNode();
			}
			
			if(startNode != destination)
			{
				bool CanGoReuse = false;
				if(LastCanGoSolution != null
				&& LastCanGoSolution[0] == startNode
				&& LastCanGoSolution[LastCanGoSolution.Length - 1] == destination)
				{
					Solution = LastCanGoSolution;
					LastCanGoSolution = null;
					CanGoReuse = true;
				}
				else
				{
					Solution = m_Graph.m_PathFinder.FindPath(startNode, destination);
				}
				
				if(Solution != null)
				{
					float[] cumulatedCosts = new float[Solution.Length];
					if (CanGoReuse)
					{
						cumulatedCosts[0] = 0;
						for (int i = 1; i < Solution.Length; ++i)
						{
							lwAstarNode.lwAstarLink link = Solution[i-1].GetLinkTo(Solution[i]);
							cumulatedCosts[i] = cumulatedCosts[i - 1] + ( link.cost * link.costMultiplier + link.weight * link.weightMultiplier );
						}
					}
					else
					{
						for (int i = 0; i < Solution.Length; ++i)
						{
							cumulatedCosts[i] = Solution[i].WeightedGlobalCost;
						}
					}
					m_nStep = 0;
					bool bStepChanged = true;

					float totalCost = cumulatedCosts[Solution.Length - 1];
					cumulatedCost = 0;
					
					while (totalCost - cumulatedCost > Speed * deltaTime)
					{
						cumulatedCost = Mathf.Clamp(cumulatedCost + Speed * deltaTime, 0, totalCost);
						while (m_nStep < Solution.Length - 1 && cumulatedCosts[m_nStep + 1] < cumulatedCost)
						{
							m_nStep++;
							if (Solution[m_nStep].NodePassed != null)
								Solution[m_nStep].NodePassed(Solution[m_nStep], this);
							if (PassedNode != null)
								PassedNode(Solution[m_nStep], this);
							bStepChanged = true;
						}

						while (m_nStep > 0 && cumulatedCosts[m_nStep] > cumulatedCost)
						{
							m_nStep--;
							if (Solution[m_nStep].NodePassed != null)
								Solution[m_nStep].NodePassed(Solution[m_nStep], this);
							if (PassedNode != null)
								PassedNode(Solution[m_nStep], this);
							bStepChanged = true;
						}

						if (bStepChanged)
						{
							lwAstarNode.lwAstarLink link = Solution[m_nStep].GetLinkTo(Solution[m_nStep + 1]);
							m_fCurrentSpeedRatio = 1.0f/( link.weight * link.weightMultiplier );
							
							if (m_UpdateDirectionDelegate != null)
								m_UpdateDirectionDelegate(Solution[m_nStep + 1].transform.position - Solution[m_nStep].transform.position);

							bStepChanged = false;
						}
						m_fCostTraveledOnCurrentLink = cumulatedCost - cumulatedCosts[m_nStep];
						float deltacost = cumulatedCosts[m_nStep + 1] - cumulatedCosts[m_nStep];

						float ratio = m_fCostTraveledOnCurrentLink / deltacost;
						if (m_UpdatePostionDelegate != null)
							m_UpdatePostionDelegate(transform, Vector3.Lerp(Solution[m_nStep].transform.localPosition, Solution[m_nStep + 1].transform.localPosition, ratio));
						else
							transform.localPosition = Vector3.Lerp(Solution[m_nStep].transform.localPosition, Solution[m_nStep + 1].transform.localPosition, ratio);

						yield return null;

						now = m_Graph.GetTime();
						deltaTime = now - fPrevTime;
						fPrevTime = now;
					}

					if (m_UpdatePostionDelegate != null)
						m_UpdatePostionDelegate(transform, Solution[Solution.Length - 1].transform.localPosition);
					else
						transform.localPosition = Solution[Solution.Length - 1].transform.localPosition;


					m_nStep = Solution.Length - 1;
					m_fCostTraveledOnCurrentLink = 0;
					m_fCurrentSpeedRatio = 1;
					if (Solution[m_nStep].NodePassed != null)
					{
						Solution[m_nStep].NodePassed(Solution[m_nStep], this);
					}
					if (PassedNode!=null)
					{
						PassedNode(Solution[m_nStep], this);
					}
				}
				else
				{
					Debug.Log("no path allow me to go to destination ... aborting");
				}
			}
		}
		m_bIsWalking = false;
	}

	void OnDrawGizmos()
	{
		Gizmos.color = Color.yellow;
		Gizmos.DrawSphere(transform.position, 15f);
	
		if( Solution!=null && Solution.Length>0 )
		{
			for(int i = 0; i<Solution.Length; ++i)
			{
				float ratio = (float)i / (float)Solution.Length;
				Color c = Color.Lerp(Color.green, Color.blue, ratio);
				c.a = 0.9f;
				Gizmos.color = c;
				Gizmos.DrawSphere(Solution[i].transform.position, 10);
				if(i<Solution.Length-1)
					Gizmos.DrawLine(Solution[i].transform.position, Solution[i+1].transform.position);
			}
		}
	}
}
